﻿/*
 * Software created by Bert Bierman
 * 
 * Demonstrating uses of C# to control the NAO robot
 * 
 * Questions/remarks email: bert@produxi.nl
 * 
 * 26 januari 2011
 * */

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Aldebaran.Proxies;
using System.Collections;
using System.IO;
using System.Threading;

namespace DemoNAOQINet
{
    public partial class FrmDemoNAQI : Form
    {
        // Proxy to motion
        MotionProxy _motion = null;

        public FrmDemoNAQI()
        {
            InitializeComponent();
            lblWarning.Text = "";
        }

        private string mIP = "127.0.0.1";
        private int mPort = 9559;

        #region ConnectTab

        // Proces the click on the button
        private void btnConnect_Click(object sender, EventArgs e)
        {
            if (txtServer.Text.Trim().Length > 0)
            {
                // Parse the connectionstring
                string[] parts = txtServer.Text.Split(":".ToCharArray());
                if (parts.Length > 1)
                {
                    mIP = parts[0];
                    mPort = int.Parse(parts[1]);
                }
                else
                {
                    lblWarning.Text = "Connection string has form: 10.10.10.10:9999";
                    return;
                }
            }
            else
            {
                lblWarning.Text = "Connection string has form: 10.10.10.10:9999";
                return;
            }

            ProcessConnect();
        }

        private bool NAOConnected = false;
        private void ProcessConnect()
        {
            // Test if a NAO is present
            if (NAOTest() == true)
            {
                // Connected, set color
                lblServerConnected.BackColor = Color.Green;
                NAOConnected = true;
                btnConnect.Text = "Connected";
            }
            else
            {
                lblWarning.Text = "NAO not connected using " + txtServer.Text;
                lblServerConnected.BackColor = Color.Red;
                NAOConnected = false;
                btnConnect.Text = "Connect";
            }

            // Set the buttons
            btnConnect.Enabled = NAOConnected;
            btnExecute.Enabled = NAOConnected;
            btnFind.Enabled = NAOConnected;
            btnPlay.Enabled = NAOConnected;
            btnFindFile.Enabled = NAOConnected;
            btnProces.Enabled = NAOConnected;

        }

        private bool NAOTest()
        {
            try
            {
                // just try a proxy. if no NAO is connected this will not work
                _motion = new MotionProxy(mIP, mPort);
                NAOConnected = true;
            }
            catch
            {
                return false;
            }

            return true;
        }

        private void txtServer_TextChanged(object sender, EventArgs e)
        {
            // Reser warning if user is typing content
            lblWarning.Text = "";
        }

        #endregion

        #region MovementTab

        private void btnFindFile_Click(object sender, EventArgs e)
        {
            // Search for a file
            OpenFileDialog ofd = new OpenFileDialog();
            ofd.Multiselect = false;
            ofd.InitialDirectory = Environment.CurrentDirectory;

            DialogResult dr = ofd.ShowDialog();

            if (dr == DialogResult.OK)
            {
                txtFilename.Text = ofd.SafeFileName;
                mFileWithPath = ofd.FileName;
            }
        }

        private string mFileWithPath = "";
        private void btnProces_Click(object sender, EventArgs e)
        {
            if (NAOConnected == true)
            {
                // Create proxy
                if (_motion == null)
                {
                    _motion = new MotionProxy(mIP, mPort);
                }

                if (_motion != null)
                {
                    // switch all motors on
                    SetMotors("Body", 1.0F);

                    ArrayList data = null;

                    // test if file exists
                    if (File.Exists(mFileWithPath) == true)
                    {
                        // read data from xml-file
                        // file contains one or more positions of one or more joints
                        data = (ArrayList)XmlUtil.ReadFromXmlFile(typeof(ArrayList), mFileWithPath);

                        if (data != null)
                        {
                            List<string> joints = new List<string>();
                            List<float> positions = new List<float>();
                            // Get number of positions
                            int nPositions = ((ArrayList)((ArrayList)data[0])[1]).Count;

                            for (int count = 0; count < nPositions; count++)
                            {
                                lblValue.Text = count + "/" + (nPositions - 1).ToString();
                                Application.DoEvents();

                                // Get data from joints and positions for this position
                                joints.Clear();
                                positions.Clear();
                                for (int i = 0; i < data.Count; i++)
                                {
                                    string name = (string)((ArrayList)data[i])[0];
                                    joints.Add(name);

                                    ArrayList pos = ((ArrayList)data[i]);
                                    ArrayList posdata = (ArrayList)pos[1];
                                    positions.Add((float)posdata[count]);

                                    Console.WriteLine(name + "  " + posdata[count]);

                                }
                                Console.Write("Execute  " + count);

                                // Start the movement
                                int MotionID = _motion.post.angleInterpolationWithSpeed(joints, positions, 0.4F);
                                // Wait until finished
                                while (_motion.isRunning(MotionID) == true)
                                {
                                    Console.WriteLine("Wait");
                                    Thread.Sleep(100);
                                }
                                Console.WriteLine("Finised  " + count);
                            }
                        }
                    }
                    // switch all motors off
                    SetMotors("Body", 0.0F);
                }
                else
                {
                    MessageBox.Show("First connect with NAO before starting replay", "NAO not connected", MessageBoxButtons.OK);
                }


            }
            else
            {
                MessageBox.Show("Connect to NAO first!");
            }
        }

        #endregion

        #region BehaviourTab
        private void btnFind_Click(object sender, EventArgs e)
        {
            // Find all behaviours on the NAO and put them in the listbox
            BehaviorManagerProxy BehaveProxy = new BehaviorManagerProxy(mIP, mPort);
            List<string> lijst = BehaveProxy.getInstalledBehaviors();
            lbBehaviour.Items.AddRange(lijst.ToArray());
        }

        private void btnExecute_Click(object sender, EventArgs e)
        {
            // Find behaviour selected
            string mName = "";
            if (lbBehaviour.SelectedItem != null)
            {
                mName = lbBehaviour.SelectedItem.ToString();
            }
            if (mName.Length > 0)
            {
                PlayBehaviour(mName);
            }
            else
            {
                MessageBox.Show("Select behaviour first");
            }

        }

        private void PlayBehaviour(string mName)
        {
            SetMotors("Body", 1.0F);

            BehaviorManagerProxy BehaveProxy = null;
            try
            {
                BehaveProxy = new BehaviorManagerProxy(mIP, mPort);

                // Test if behaviour is present
                if (BehaveProxy.isBehaviorInstalled(mName) == true)
                {
                    // If behaviour is running then stop it
                    while (BehaveProxy.isBehaviorRunning(mName) == true)
                    {
                        BehaveProxy.stopBehavior(mName);
                        System.Diagnostics.Debug.WriteLine(mName + " was running");
                    }
                    // Start behaviour
                    int id = BehaveProxy.post.runBehavior(mName);
                    while (BehaveProxy.isRunning(id) == true)
                    {
                        Application.DoEvents();
                        Thread.Sleep(100);
                    }

                }
                else
                {
                    System.Diagnostics.Debug.WriteLine(mName += " not installed");
                }
            }
            catch (Exception exc)
            {
                System.Diagnostics.Debug.WriteLine(exc.Message);
            }

            SetMotors("Body", 0.0F);

        }

        #endregion

        #region PlayAudioTab
        private void btnPlay_Click(object sender, EventArgs e)
        {
            string filename = txtNAOMap.Text;
            // Concatenate map and filename
            if (filename.EndsWith("/") == false)
            {
                filename += "/";
            }
            filename += txtSoundFilename.Text;

            // Play the file
            if (filename.Length > 0)
            {
                PlayAudio(filename);
            }
        }

        private void PlayAudio(string filename)
        {
            try
            {
                // Start playing and wait until finished
                AudioPlayerProxy ap = new AudioPlayerProxy(mIP, mPort);
                int id = ap.post.playFile(filename.Trim());
                while (ap.isRunning(id) == true)
                {
                    Application.DoEvents();
                    Thread.Sleep(100);
                }
                ap.Dispose();
            }
            catch (Exception ex)
            {
                MessageBox.Show("Is file " + filename + " wel aanwezig?" + ex.Message);
            }
        }

        #endregion
        
        #region Helpers
		
        private void SetMotors(string which, float value)
        {
            if (_motion == null)
            {
                _motion = new MotionProxy(mIP, mPort);
            }
            if (_motion != null)
            {
                // switch all motors on
                _motion.setStiffnesses(which, value);
            }
        }

        private void FrmDemoNAQI_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (_motion != null)
            {
                _motion.Dispose();
            }
        }

        private void btnStart_Click(object sender, EventArgs e)
        {
            SetMotors("Body", 1.0F);
        }

        private void btnStop_Click(object sender, EventArgs e)
        {
            SetMotors("Body", 0.0F);
        }

	    #endregion
    
    }
}
